#include "stm32f4xx.h"                  // Device header
#include "stm32f4xx_conf.h"
#include "FuzzyPID.h"

/*====================模糊化====================*/

/*
 *函数简介:区间映射函数
 *参数说明:浮点型数据及数据所属区间
 *返回类型:将数据映射到论域的值
 *备注:默认论域[-3,3]
 */
float FuzzyPID_Map(float Input,float *Interval)
{
	return 6*(Input-Interval[0])/(Interval[1]-Interval[0])-3;//区间映射
}

/*
 *函数简介:隶属度函数
 *参数说明:e或ec映射到论域的值(float型)
 *返回类型:e或ec的隶属度(FuzzyPID_InputMembership结构体)
 *备注:默认论域[-3,3]
 */
FuzzyPID_InputMembership FuzzyPID_GetMembership(float Input)
{
	FuzzyPID_InputMembership OUT;
	OUT.Membership1=(FuzzyPID_Domain)(int)Input;//获取隶属1
	OUT.Membership2=(FuzzyPID_Domain)((int)Input+1);//获取隶属2
	OUT.Degree2=Input-(int)Input;//获取隶属度2
	OUT.Degree1=1-OUT.Degree2;//获取隶属度1
	return OUT;
}

/*
 *函数简介:模糊化函数
 *参数说明:Fuzzy_PID结构体
 *返回类型:无
 *备注:默认论域[-3,3]
 */
void FuzzyPID_Fuzzification(Fuzzy_PID *Fuzzy_PIDStructure)
{
	Fuzzy_PIDStructure->e_Membership=FuzzyPID_GetMembership(FuzzyPID_Map(Fuzzy_PIDStructure->e,Fuzzy_PIDStructure->e_Interval));//获取误差隶属度
	Fuzzy_PIDStructure->ec_Membership=FuzzyPID_GetMembership(FuzzyPID_Map(Fuzzy_PIDStructure->ec,Fuzzy_PIDStructure->ec_Interval));//获取误差变化率隶属度
}

/*====================模糊推理====================*/

/*推理表*/
FuzzyPID_Domain Kp_Table[7][7]={{PB,PB,PM,PM,PS,ZO,ZO},
								{PB,PB,PM,PS,PS,ZO,NS},
								{PM,PM,PM,PS,ZO,NS,NS},
								{PM,PM,PS,ZO,NS,NM,NM},
								{PS,PS,ZO,NS,NS,NM,NM},
								{PS,ZO,NS,NM,NM,NM,NB},
								{ZO,ZO,NM,NM,NM,NB,NB}};//Kp推理表
									
FuzzyPID_Domain Ki_Table[7][7]={{NB,NB,NM,NM,NS,ZO,ZO},
								{NB,NB,NM,NS,NS,ZO,ZO},
								{NB,NM,NS,NS,ZO,PS,PS},
								{NM,NM,NS,ZO,PS,PM,PM},
								{NM,NS,ZO,PS,PS,PM,PB},
								{ZO,ZO,PS,PS,PM,PB,PB},
								{ZO,ZO,PS,PM,PM,PB,PB}};//Ki推理表
									
FuzzyPID_Domain Kd_Table[7][7]={{PS,NS,NB,NB,NB,NM,PS},
								{PS,NS,NB,NM,NM,NS,ZO},
								{ZO,NS,NM,NM,NS,NS,ZO},
								{ZO,NS,NS,NS,NS,NS,ZO},
								{ZO,ZO,ZO,ZO,ZO,ZO,ZO},
								{PB,NS,PS,PS,PS,PS,PB},
								{PB,PM,PM,PM,PS,PS,PB}};//Kd推理表

/*
 *函数简介:模糊推理
 *参数说明:Fuzzy_PID结构体
 *返回类型:无
 *备注:无
 */
void FuzzyPID_Reasoning(Fuzzy_PID *Fuzzy_PIDStructure)
{
	//Kp
	(Fuzzy_PIDStructure->Kp_Membership).Membership1=Kp_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership1+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership1+3];
	(Fuzzy_PIDStructure->Kp_Membership).Membership2=Kp_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership1+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership2+3];
	(Fuzzy_PIDStructure->Kp_Membership).Membership3=Kp_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership2+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership1+3];
	(Fuzzy_PIDStructure->Kp_Membership).Membership4=Kp_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership2+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership2+3];//获取Kp四个隶属
	(Fuzzy_PIDStructure->Kp_Membership).Degree1=(Fuzzy_PIDStructure->e_Membership).Degree1*(Fuzzy_PIDStructure->ec_Membership).Degree1;
	(Fuzzy_PIDStructure->Kp_Membership).Degree2=(Fuzzy_PIDStructure->e_Membership).Degree1*(Fuzzy_PIDStructure->ec_Membership).Degree2;
	(Fuzzy_PIDStructure->Kp_Membership).Degree3=(Fuzzy_PIDStructure->e_Membership).Degree2*(Fuzzy_PIDStructure->ec_Membership).Degree1;
	(Fuzzy_PIDStructure->Kp_Membership).Degree4=(Fuzzy_PIDStructure->e_Membership).Degree2*(Fuzzy_PIDStructure->ec_Membership).Degree2;//获取Kp四个隶属度
	
	//Ki
	(Fuzzy_PIDStructure->Ki_Membership).Membership1=Ki_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership1+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership1+3];
	(Fuzzy_PIDStructure->Ki_Membership).Membership2=Ki_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership1+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership2+3];
	(Fuzzy_PIDStructure->Ki_Membership).Membership3=Ki_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership2+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership1+3];
	(Fuzzy_PIDStructure->Ki_Membership).Membership4=Ki_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership2+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership2+3];//获取Ki四个隶属
	(Fuzzy_PIDStructure->Ki_Membership).Degree1=(Fuzzy_PIDStructure->e_Membership).Degree1*(Fuzzy_PIDStructure->ec_Membership).Degree1;
	(Fuzzy_PIDStructure->Ki_Membership).Degree2=(Fuzzy_PIDStructure->e_Membership).Degree1*(Fuzzy_PIDStructure->ec_Membership).Degree2;
	(Fuzzy_PIDStructure->Ki_Membership).Degree3=(Fuzzy_PIDStructure->e_Membership).Degree2*(Fuzzy_PIDStructure->ec_Membership).Degree1;
	(Fuzzy_PIDStructure->Ki_Membership).Degree4=(Fuzzy_PIDStructure->e_Membership).Degree2*(Fuzzy_PIDStructure->ec_Membership).Degree2;//获取Ki四个隶属度

	//Kd
	(Fuzzy_PIDStructure->Kd_Membership).Membership1=Kd_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership1+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership1+3];
	(Fuzzy_PIDStructure->Kd_Membership).Membership2=Kd_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership1+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership2+3];
	(Fuzzy_PIDStructure->Kd_Membership).Membership3=Kd_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership2+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership1+3];
	(Fuzzy_PIDStructure->Kd_Membership).Membership4=Kd_Table[(int)(Fuzzy_PIDStructure->e_Membership).Membership2+3][(int)(Fuzzy_PIDStructure->ec_Membership).Membership2+3];//获取Kd四个隶属
	(Fuzzy_PIDStructure->Kd_Membership).Degree1=(Fuzzy_PIDStructure->e_Membership).Degree1*(Fuzzy_PIDStructure->ec_Membership).Degree1;
	(Fuzzy_PIDStructure->Kd_Membership).Degree2=(Fuzzy_PIDStructure->e_Membership).Degree1*(Fuzzy_PIDStructure->ec_Membership).Degree2;
	(Fuzzy_PIDStructure->Kd_Membership).Degree3=(Fuzzy_PIDStructure->e_Membership).Degree2*(Fuzzy_PIDStructure->ec_Membership).Degree1;
	(Fuzzy_PIDStructure->Kd_Membership).Degree4=(Fuzzy_PIDStructure->e_Membership).Degree2*(Fuzzy_PIDStructure->ec_Membership).Degree2;//获取Kd四个隶属度

}

/*====================去模糊化====================*/

/*
 *函数简介:区间逆映射函数
 *参数说明:浮点型数据及数据所属区间
 *返回类型:将论域的值映射到数据
 *备注:默认论域[-3,3]
 */
float FuzzyPID_InverseMap(float Input,float *Interval)
{
	return (Interval[1]-Interval[0])*(Input+3)/6+Interval[0];//区间映射
}

/*
 *函数简介:去模糊化函数
 *参数说明:Fuzzy_PID结构体
 *返回类型:无
 *备注:获取Kp、Ki和Kd的值
 */
void FuzzyPID_Defuzzification(Fuzzy_PID *Fuzzy_PIDStructure)
{
	float Temp;
	
	Temp=Fuzzy_PIDStructure->Kp_Membership.Membership1*Fuzzy_PIDStructure->Kp_Membership.Degree1+Fuzzy_PIDStructure->Kp_Membership.Membership2*Fuzzy_PIDStructure->Kp_Membership.Degree2+Fuzzy_PIDStructure->Kp_Membership.Membership3*Fuzzy_PIDStructure->Kp_Membership.Degree3+Fuzzy_PIDStructure->Kp_Membership.Membership4*Fuzzy_PIDStructure->Kp_Membership.Degree4;//Kp在论域中的值
	Fuzzy_PIDStructure->Kp=FuzzyPID_InverseMap(Temp,Fuzzy_PIDStructure->Kp_Interval);//获取Kp的值
	Temp=Fuzzy_PIDStructure->Ki_Membership.Membership1*Fuzzy_PIDStructure->Ki_Membership.Degree1+Fuzzy_PIDStructure->Ki_Membership.Membership2*Fuzzy_PIDStructure->Ki_Membership.Degree2+Fuzzy_PIDStructure->Ki_Membership.Membership3*Fuzzy_PIDStructure->Ki_Membership.Degree3+Fuzzy_PIDStructure->Ki_Membership.Membership4*Fuzzy_PIDStructure->Ki_Membership.Degree4;//Ki在论域中的值
	Fuzzy_PIDStructure->Ki=FuzzyPID_InverseMap(Temp,Fuzzy_PIDStructure->Ki_Interval);//获取Ki的值
	Temp=Fuzzy_PIDStructure->Kd_Membership.Membership1*Fuzzy_PIDStructure->Kd_Membership.Degree1+Fuzzy_PIDStructure->Kd_Membership.Membership2*Fuzzy_PIDStructure->Kd_Membership.Degree2+Fuzzy_PIDStructure->Kd_Membership.Membership3*Fuzzy_PIDStructure->Kd_Membership.Degree3+Fuzzy_PIDStructure->Kd_Membership.Membership4*Fuzzy_PIDStructure->Kd_Membership.Degree4;//Kd在论域中的值
	Fuzzy_PIDStructure->Kd=FuzzyPID_InverseMap(Temp,Fuzzy_PIDStructure->Kd_Interval);//获取Kd的值
}

/*====================模糊PID====================*/

/*
 *函数简介:模糊控制器
 *参数说明:Fuzzy_PID结构体
 *返回类型:无
 *备注:无
 */
void FuzzyPID_FuzzyController(Fuzzy_PID *Fuzzy_PIDStructure)
{
	FuzzyPID_Fuzzification(Fuzzy_PIDStructure);
	FuzzyPID_Reasoning(Fuzzy_PIDStructure);
	FuzzyPID_Defuzzification(Fuzzy_PIDStructure);
}

/*
 *函数简介:模糊PID
 *参数说明:Fuzzy_PID结构体
 *返回类型:无
 *备注:在调用函数前应先获取NowValue(现有值)
 */
void FuzzyPID_PID(Fuzzy_PID *Fuzzy_PIDStructure,float NowValue)
{
	Fuzzy_PIDStructure->e_1=Fuzzy_PIDStructure->e;//获取上一次误差
	Fuzzy_PIDStructure->e=Fuzzy_PIDStructure->NeedValue-NowValue;//获取当前误差
	if(Fuzzy_PIDStructure->e_low<Fuzzy_PIDStructure->e && Fuzzy_PIDStructure->e<Fuzzy_PIDStructure->e_up)//误差为0检测
		Fuzzy_PIDStructure->e=0;
	Fuzzy_PIDStructure->Sum_e+=Fuzzy_PIDStructure->e;//获取误差积分
	Fuzzy_PIDStructure->Del_e=Fuzzy_PIDStructure->e-Fuzzy_PIDStructure->e_1;//获取误差差分
	Fuzzy_PIDStructure->ec=Fuzzy_PIDStructure->Del_e/Fuzzy_PIDStructure->t;//获取误差变化率
	
	FuzzyPID_FuzzyController(Fuzzy_PIDStructure);//模糊控制器获取Kp、Ki、Kd
	Fuzzy_PIDStructure->POut=Fuzzy_PIDStructure->Kp*Fuzzy_PIDStructure->e;//获取比例输出
	Fuzzy_PIDStructure->IOut=Fuzzy_PIDStructure->Ki*Fuzzy_PIDStructure->Sum_e;//获取积分输出
	Fuzzy_PIDStructure->DOut=Fuzzy_PIDStructure->Kd*Fuzzy_PIDStructure->Del_e;//获取微分输出
	Fuzzy_PIDStructure->OUT=Fuzzy_PIDStructure->POut+Fuzzy_PIDStructure->IOut+Fuzzy_PIDStructure->DOut;//获取总输出
	
	if(Fuzzy_PIDStructure->OUT<Fuzzy_PIDStructure->OUT_low)//输出限幅
		Fuzzy_PIDStructure->OUT=Fuzzy_PIDStructure->OUT_low;
	if(Fuzzy_PIDStructure->OUT>Fuzzy_PIDStructure->OUT_up)
		Fuzzy_PIDStructure->OUT=Fuzzy_PIDStructure->OUT_up;
}
